
#include "../common/error.h"
#include "../common/sparse_matrix.h"
#include "../common/dynamic_arr.h"
#include "../common/dynamic_int_arr.h"
#include "../common/mem.h"

sparse_matrix_arr* allocate_sparse_matrix_arr(int n)
{
	sparse_matrix_arr* matrix;

	matrix = mem_alloc_and_zero(1, sizeof(sparse_matrix_arr));
	validate_allocation(matrix, "a sparse-matrix structure");
	
	matrix->n = n;

	/*  +1 for the pointer to the end of the values */
	matrix->rowptr = mem_alloc_and_zero(n+1, sizeof(int));
	validate_allocation(matrix->rowptr, "sparse-matrix internal structure");

	return matrix;
}

void free_sparse_matrix_arr(sparse_matrix_arr* matrix)
{
	if (matrix->values != 0)
		mem_free(matrix->values);

	if (matrix->rowptr != 0)
		mem_free(matrix->rowptr);

	if (matrix->colind != 0)
		mem_free(matrix->colind);

	mem_free(matrix);
}

void mult_sparse_arr(const sparse_matrix_arr *A, const elem *v, elem* result)
{
	int n;
	int i,j;
	int row_start;
	int row_end;
	elem* values;
	int* indexes;
	int index;

	n = A->n;

	/* Initialize result */
	for (i = 0; i < n; i++)
		result[i] = 0;

	indexes = A->colind;
	values = A->values;

	for (i = 0; i < n; i++)
	{
		row_start = A->rowptr[i];
		row_end = A->rowptr[i + 1];

		for (j = row_start; j < row_end; j++)
		{
			index = indexes[j];
			result[i] += v[index] * values[j];
		}
	}
}

sparse_matrix_arr* sparse_matrix__read_from_file(const char* from, int n)
{
	sparse_matrix_arr* result;

	/*  Try to open file */
	FILE* file = fopen(from, "r");

	/*  Check for an error */
	if (file == NULL)
		fatal_error("Could not open matrix file!");

	result = sparse_matrix__read_from_stream(file, n);
	fclose(file);

	if (result->n == 0)
		fatal_error("Matrix should be bigger than 0.");

	return result;
}

sparse_matrix_arr* sparse_matrix__read_from_stream(FILE* stream, int n)
{
	sparse_matrix_arr* matrix;

	int row, col;
	elem d;
	dynamic_arr* values_arr;
	dynamic_int_arr* col_index_arr;

	/*  If "n==-1" then read the first number as the matrix's size */
	if (n == -1 && fscanf(stream, "%i", &n) != 1)
		fatal_error("Could not read leading matrix size!");

	if (n < 0)
		fatal_error("Matrix dimension cannot be negative!");

	matrix = allocate_sparse_matrix_arr(n);
	values_arr = dynamic_arr__allocate();
	col_index_arr = dynamic_int_arr__allocate();

	for (row = 0; row < n; row++)
	{
		matrix->rowptr[row] = values_arr->count;
		for (col = 0; col < n; col++)
		{
			/* Read value and report error if needed */
			if (fscanf(stream, elem_scan_format, &d) != 1)
				fatal_error("Bad matrix value encountered");

			if (d != 0)
			{
				dynamic_arr__add(values_arr, d);
				dynamic_int_arr__add(col_index_arr, col);
			}
		}
	}

	/*  Set a pointer to the end of the values */
	matrix->rowptr[n] = values_arr->count;

	/*  Copy values to the new matrix */

	matrix->values = mem_alloc(values_arr->count, sizeof(values_arr->values[0]));
	validate_allocation(matrix->values, "sub-matrix internal structure");
	memcpy(matrix->values, values_arr->values,
		values_arr->count * sizeof(values_arr->values[0]));

	matrix->colind = mem_alloc(col_index_arr->count, sizeof(col_index_arr->values[0]));
	validate_allocation(matrix->colind, "sub-matrix internal structure");
	memcpy(matrix->colind, col_index_arr->values,
		col_index_arr->count * sizeof(col_index_arr->values[0]));

	dynamic_arr__free(values_arr);
	dynamic_int_arr__free(col_index_arr);

	return matrix;
}

sparse_matrix_arr* sparse_matrix__make_sub_matrix(const sparse_matrix_arr* matrix,
												  int* indices, int indices_count)
{
	sparse_matrix_arr* sub_matrix;
	int i, j, columns_index;
	int row, row_start, row_end;
	dynamic_arr* values;
	dynamic_int_arr* columns;

	sub_matrix = allocate_sparse_matrix_arr(indices_count);

	values = dynamic_arr__allocate();
	columns = dynamic_int_arr__allocate();

	for (i = 0; i < indices_count; i++)
	{
		if (indices[i] < 0 || indices[i] >= matrix->n)
			fatal_error("Bad indexes. Out of matrix range.");

		sub_matrix->rowptr[i] = values->count;

		j = 0;
		row = indices[i];
		row_start = matrix->rowptr[row];
		row_end = matrix->rowptr[row + 1];
		columns_index = row_start;

		while (columns_index < row_end)
		{
			/* Run J up to the next column index */
			for (;
				j < indices_count && indices[j] < matrix->colind[columns_index];
				j++);

			/* End of indices? */
			if (j < indices_count)
			{
				/* Has the index passed the column index? */
				if (indices[j] == matrix->colind[columns_index])
				{
					dynamic_arr__add(values, matrix->values[columns_index]);
					dynamic_int_arr__add(columns, j);
				}
			}

			columns_index++;
		}
	}

	sub_matrix->rowptr[indices_count] = values->count;

	/*  Copy values to the new matrix */

	sub_matrix->values = mem_alloc(values->count, sizeof(values->values[0]));
	validate_allocation(sub_matrix->values, "sub-matrix internal structure");
	memcpy(sub_matrix->values, values->values, values->count * sizeof(values->values[0]));

	sub_matrix->colind = mem_alloc(columns->count, sizeof(columns->values[0]));
	validate_allocation(sub_matrix->colind, "sub-matrix internal structure");
	memcpy(sub_matrix->colind, columns->values, columns->count * sizeof(columns->values[0]));

	dynamic_arr__free(values);
	dynamic_int_arr__free(columns);

	return sub_matrix;
}

void sparse_matrix__print_integers(const sparse_matrix_arr* matrix, FILE* stream)
{
	sparse_matrix__print(matrix, stream, "%0.lf ");
}

void sparse_matrix__print(const sparse_matrix_arr* matrix, FILE* stream, const char* element_format)
{
	int row, col;
	int column_index;
	int columns_end;

	fprintf(stream, "%d\n", matrix->n);

	for (row = 0; row < matrix->n; row++)
	{
		column_index = matrix->rowptr[row];
		columns_end = matrix->rowptr[row + 1];

		for (col = 0; col < matrix->n; col++)
		{
			while (column_index < columns_end && matrix->colind[column_index] < col)
				column_index++;

			if (column_index < columns_end && matrix->colind[column_index] == col)
				fprintf(stream, element_format, matrix->values[column_index]);
			else
				fprintf(stream, element_format, 0.0);
		}

		fprintf(stream, "\n");
	}
}

elem* sparse_matrix__to_array(const sparse_matrix_arr* matrix)
{
	int row, col;
	int column_index;
	int columns_end;
	elem* result;
	int n;

	n = matrix->n;
	result = mem_alloc(n * n, sizeof(elem));

	for (row = 0; row < n; row++)
	{
		column_index = matrix->rowptr[row];
		columns_end = matrix->rowptr[row + 1];

		for (col = 0; col < n; col++)
		{
			while (column_index < columns_end && matrix->colind[column_index] < col)
				column_index++;

			if (column_index < columns_end && matrix->colind[column_index] == col)
				result[row * n + col] = matrix->values[column_index];
			else
				result[row * n + col] = 0;
		}
	}

	return result;
}

elem sparse_matrix__sum_row(const sparse_matrix_arr* matrix, int row)
{
	elem result;
	int column_index, columns_end;

	result = 0;
	columns_end = matrix->rowptr[row + 1];

	for (column_index = matrix->rowptr[row];
		column_index < columns_end; column_index++)
	{
		result += matrix->values[column_index];
	}

	return result;
}

elem sparse_matrix__mlpl_matrix_row_times_vector(const sparse_matrix_arr* matrix,
												 int row, const elem* vector)
{
	elem result;
	int column_index;
	int row_start, row_end;
	int index;
	result = 0;

	row_start = matrix->rowptr[row];
	row_end = matrix->rowptr[row + 1];

	for (column_index = row_start; column_index < row_end; column_index++)
	{
		index = matrix->colind[column_index];
		result += matrix->values[column_index] * vector[index];
	}

	return result;
}

void sparse_matrix__mlpl_matrix_times_vector(const sparse_matrix_arr* matrix,
											 const elem* vector, elem* result)
{
	int i;
	int n;

	n = matrix->n;

	for (i = 0; i < n; i++)
	{
		result[i] = sparse_matrix__mlpl_matrix_row_times_vector(matrix, i,
			vector);
	}
}

static int sparse_matrix__get_cell__last_row = -1;
static int sparse_matrix__get_cell__last_val_index = -1;

elem sparse_matrix__get_cell(const sparse_matrix_arr* matrix, int row, int col)
{
	int* columns;
	int row_start;
	int row_end;
	int val_index;

	row_start = matrix->rowptr[row];
	row_end = matrix->rowptr[row + 1];
	columns = matrix->colind;

	/*  The previous "get" was from the same row */
	if (row == sparse_matrix__get_cell__last_row)
	{
		/*  Search forward or back? */
		if (columns[sparse_matrix__get_cell__last_val_index] < col)
		{
			for (val_index = sparse_matrix__get_cell__last_val_index;
				(val_index < row_end) && (columns[val_index] < col);
				val_index++);
		}
		else /*  backwards */
		{
			for (val_index = sparse_matrix__get_cell__last_val_index;
				(val_index >= row_start) && (columns[val_index] > col);
				val_index--);

			if (val_index < row_start)
				return 0;
		}
	}
	else /*  An other row, regular search (forward, from the start) */
	{
		for (val_index = row_start;
			(val_index < row_end) && (columns[val_index] < col); val_index++);
	}

	if (val_index < row_end && columns[val_index] == col)
	{
		sparse_matrix__get_cell__last_row = row;
		sparse_matrix__get_cell__last_val_index = val_index;
		return matrix->values[val_index];
	}

	return 0;
}

dynamic_int_arr* sparse_matrix__search_for_singletons(const sparse_matrix_arr* matrix)
{
	int* rows;
	int i,n;
	/*int* result;*/
	dynamic_int_arr* indices;
	
	n = matrix->n;
	indices = dynamic_int_arr__allocate();
	rows  = matrix->rowptr;

	for (i = 0 ; i < n ; i++)
	{
		if (rows[i+1] == rows[i])
		{
			dynamic_int_arr__add(indices,i);
		}
	}
	
	/*
	result = mem_alloc(indices->count,sizeof(int));
	validate_allocation(result,"Singleton search results");
	memcpy(result, indices->values, indices->count * sizeof(int));

	dynamic_int_arr__free(indices);
	*/
	return indices;
}
